/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2016,
*  TU Dortmund - Institute of Control Theory and Systems Engineering.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the institute nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann, Otniel Rinaldo
*********************************************************************/

#include <costmap_converter/costmap_to_polygons.h>
#include <costmap_converter/misc.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)

namespace
{

/** 将直线拟合到有序点集的Douglas-Peucker算法
* @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points
*
* Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
*
* @param begin iterator pointing to the begin of the range of points
* @param end interator pointing to the end of the range of points
* @param epsilon distance criteria for removing points if it is closer to the line segment than this
* @param result the simplified polygon
*/
std::vector<geometry_msgs::Point32> douglasPeucker(std::vector<geometry_msgs::Point32>::iterator begin,
                                                   std::vector<geometry_msgs::Point32>::iterator end,
                                                   double epsilon)
{
    if (std::distance(begin, end) <= 2) return std::vector<geometry_msgs::Point32>(begin, end);
    // Find the point with the maximum distance from the line [begin, end)
    double dmax = std::numeric_limits<double>::lowest();
    std::vector<geometry_msgs::Point32>::iterator max_dist_it;
    auto last = std::prev(end);
    for (auto it = std::next(begin); it != last; ++it)
    {
        double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);
        if (d > dmax)
        {
            max_dist_it = it;
            dmax = d;
        }
    }
    if (dmax < epsilon * epsilon)
    {
        // termination criterion reached, line is good enough 达到终止条件，线路足够好
        std::vector<geometry_msgs::Point32> result;
        result.push_back(*begin);
        result.push_back(*last);
        return result;
    }
    // Recursive calls for the two splitted parts
    auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);
    auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);
    // Combine the two lines into one line and return the merged line.
    // Note that we have to skip the first point of the second line, as it is duplicated above.
    // 将两行合并为一行并返回合并后的行。
    // 注意，我们必须跳过第二行的第一个点，因为它在上面是重复的。
    firstLineSimplified.insert(firstLineSimplified.end(),
                               std::make_move_iterator(std::next(secondLineSimplified.begin())),
                               std::make_move_iterator(secondLineSimplified.end()));
    return firstLineSimplified;
}

} // end namespace

namespace costmap_converter
{

CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
{
    costmap_ = NULL;
    dynamic_recfg_ = NULL;
    neighbor_size_x_ = neighbor_size_y_ = -1;
    offset_x_ = offset_y_ = 0.;
}

CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH() { delete dynamic_recfg_; }

void CostmapToPolygonsDBSMCCH::initialize(ros::NodeHandle nh)
{
    costmap_ = NULL;
    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);
    parameter_buffered_ = parameter_;
    // setup dynamic reconfigure
    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(
            &CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);
}

void CostmapToPolygonsDBSMCCH::setCostmap2D(costmap_2d::Costmap2D *costmap)
{
    if (!costmap) return;
    costmap_ = costmap;
    updateCostmap2D();
}

void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
    occupied_cells_.clear();
    if (!costmap_->getMutex())
    {
        ROS_ERROR("Cannot update costmap since the mutex pointer is null");
        return;
    }
    // get a copy of our parameters from dynamic reconfigure
    {
        boost::mutex::scoped_lock lock(parameter_mutex_);
        parameter_ = parameter_buffered_;
    }
    costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
    // allocate neighbor lookup 分配邻居查找
    int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;
    int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;
    if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_)
    {
        neighbor_size_x_ = cells_x;
        neighbor_size_y_ = cells_y;
        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);
    }
    offset_x_ = costmap_->getOriginX();
    offset_y_ = costmap_->getOriginY();
    for (auto &n : neighbor_lookup_) n.clear();
    // get indices of obstacle cells 获得障碍单元的下标
    for (std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
    {
        for (std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
        {
            int value = costmap_->getCost(i, j);
            if (value >= costmap_2d::LETHAL_OBSTACLE)
            {
                double x, y;
                costmap_->mapToWorld((unsigned int) i, (unsigned int) j, x, y);
                addPoint(x, y);
            }
        }
    }
}

void CostmapToPolygonsDBSMCCH::compute()
{
    std::vector<std::vector<KeyPoint> > clusters;
    dbScan(clusters);
    // Create new polygon container 创建新的多边形容器
    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());
    // add convex hulls to polygon container 给多边形容器添加凸壳
    // skip first cluster, since it is just noise 跳过第一个簇，因为它只是噪声
    for (std::size_t i = 1; i < clusters.size(); ++i)
    {
        polygons->push_back(geometry_msgs::Polygon());
        convexHull2(clusters[i], polygons->back());
    }
    // add our non-cluster points to the polygon container (as single points)
    // 添加我们的非聚类点到多边形容器(作为单个点)
    if (!clusters.empty())
    {
        for (auto i : clusters.front())
        {
            polygons->push_back(geometry_msgs::Polygon());
            // 将generi点类型转换为geometry_msgs::Polygon
            convertPointToPolygon(i, polygons->back());
        }
    }
    // replace shared polygon container
    // 替换共享多边形容器
    updatePolygonContainer(polygons);
}

void CostmapToPolygonsDBSMCCH::dbScan(std::vector<std::vector<KeyPoint> > &clusters)
{
    std::vector<bool> visited(occupied_cells_.size(), false);
    clusters.clear();
    // DB Scan Algorithm DB扫描算法
    int cluster_id = 0; // current cluster_id
    clusters.emplace_back();
    for (int i = 0; i < (int) occupied_cells_.size(); i++)
    {
        // 以前从未被访问过的关键点
        if (!visited[i])
        {
            visited[i] = true; // mark as visited
            std::vector<int> neighbors;
            // Find neighbors around the keypoint 找到关键点周围的邻居
            regionQuery(i, neighbors);
            // If not enough neighbors are found, mark as noise
            // 如果没有找到足够的邻居，标记为噪音
            if ((int) neighbors.size() < parameter_.min_pts_)
            {
                clusters[0].push_back(occupied_cells_[i]);
            }
            else
            {
                // increment current cluster_id
                ++cluster_id;
                clusters.emplace_back();
                // Expand the cluster
                clusters[cluster_id].push_back(occupied_cells_[i]);
                for (int j = 0; j < (int) neighbors.size(); j++)
                {
                    if ((int) clusters[cluster_id].size() == parameter_.max_pts_) break;
                    // keypoint has not been visited before
                    if (!visited[neighbors[j]])
                    {
                        // mark as visited
                        visited[neighbors[j]] = true;
                        std::vector<int> further_neighbors;
                        //Find more neighbors around the new keypoint
                        regionQuery(neighbors[j], further_neighbors);
//             if(further_neighbors.size() < min_pts_)
//             {	  
//               clusters[0].push_back(occupied_cells[neighbors[j]]);
//             }
//             else
                        if ((int) further_neighbors.size() >= parameter_.min_pts_)
                        {
                            // neighbors found
                            // Add these newfound P' neighbour to P neighbour vector "nb_indeces"
                            neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());
                            clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);
                        }
                    }
                }
            }
        }
    }
}

void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int> &neighbors)
{
    neighbors.clear();
    double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;
    const KeyPoint &kp = occupied_cells_[curr_index];
    int cx, cy;
    pointToNeighborCells(kp, cx, cy);
    // loop over the neighboring cells for looking up the points
    const int offsets[9][2] = {
            {-1, -1}, {0,  -1}, {1,  -1},
            {-1, 0}, {0,  0}, {1,  0},
            {-1, 1}, {0,  1}, {1,  1}
    };
    for (auto offset : offsets)
    {
        int idx = neighborCellsToIndex(cx + offset[0], cy + offset[1]);
        if (idx < 0 || idx >= int(neighbor_lookup_.size())) continue;
        const std::vector<int> &pointIndicesToCheck = neighbor_lookup_[idx];
        for (int point_idx : pointIndicesToCheck)
        {
            // point is not a neighbor to itself
            if (point_idx == curr_index) continue;
            const KeyPoint &other = occupied_cells_[point_idx];
            double dx = other.x - kp.x;
            double dy = other.y - kp.y;
            double dist_sqr = dx * dx + dy * dy;
            if (dist_sqr <= dist_sqr_threshold) neighbors.push_back(point_idx);
        }
    }
}

bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint &p1,
                          const CostmapToPolygonsDBSMCCH::KeyPoint &p2)
{
    return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
}

// Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull
void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint> &cluster, geometry_msgs::Polygon &polygon)
{
    int k = 0;
    int n = cluster.size();
    // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
    std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);
    polygon.points.resize(2 * n);
    // lower hull
    for (int i = 0; i < n; ++i)
    {
        while (k >= 2 && cross(polygon.points[k - 2], polygon.points[k - 1], cluster[i]) <= 0) --k;
        cluster[i].toPointMsg(polygon.points[k]);
        ++k;
    }
    // upper hull
    for (int i = n - 2, t = k + 1; i >= 0; --i)
    {
        while (k >= t && cross(polygon.points[k - 2], polygon.points[k - 1], cluster[i]) <= 0) --k;
        cluster[i].toPointMsg(polygon.points[k]);
        ++k;
    }
    polygon.points.resize(k); // original
    // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
//     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost
    simplifyPolygon(polygon);
}

void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint> &cluster, geometry_msgs::Polygon &polygon)
{
    std::vector<KeyPoint> &P = cluster;
    std::vector<geometry_msgs::Point32> &points = polygon.points;
    // Sort P by x and y
    std::sort(P.begin(), P.end(), isXCoordinateSmaller);
    // the output array H[] will be used as the stack
    int i;                 // array scan index
    // Get the indices of points with min x-coord and min|max y-coord
    // 得到x轴最小和y轴最小的点的索引
    int minmin = 0, minmax;
    double xmin = P[0].x;
    // 找到x轴发生变化的位置
    for (i = 1; i < (int) P.size(); i++)
    {
        if (P[i].x != xmin) break;
    }
    minmax = i - 1;
    // 如果是最后一个点
    if (minmax == (int) P.size() - 1)
    {
        // degenerate case: all x-coords == xmin
        points.emplace_back();
        P[minmin].toPointMsg(points.back());
        // a nontrivial segment
        if (P[minmax].y != P[minmin].y)
        {
            points.emplace_back();
            P[minmax].toPointMsg(points.back());
        }
        // add polygon endpoint
        points.emplace_back();
        P[minmin].toPointMsg(points.back());
        return;
    }
    // Get the indices of points with max x-coord and min|max y-coord
    // 获取x坐标最大值和y坐标最小值的点的索引
    int maxmin, maxmax = (int) P.size() - 1;
    double xmax = P.back().x;
    for (i = P.size() - 2; i >= 0; i--)
    {
        if (P[i].x != xmax) break;
    }
    maxmin = i + 1;
    // Compute the lower hull on the stack H push minmin point onto stack
    // 构建下凸包，从左跑到右，由下面通过
    points.emplace_back();
    P[minmin].toPointMsg(points.back());
    i = minmax;
    while (++i <= maxmin)
    {
        // the lower line joins P[minmin]  with P[maxmin] ignore P[i] above or on the lower line
        if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin) continue;
        // there are at least 2 points on the stack
        while (points.size() > 1)
        {
            // test if P[i] is left of the line at the stack top P[i] is a new hull vertex
            // 测试P[i]是否在栈顶线的左边P[i]是一个新的船体顶点
            if (cross(points[points.size() - 2], points.back(), P[i]) > 0) break;
            // pop top point off stack
            points.pop_back();
        }
        // push P[i] onto stack
        points.emplace_back();
        P[i].toPointMsg(points.back());
    }
    // Next, compute the upper hull on the stack H above the bottom hull
    // 构建上凸包，从右到左，由上面通过
    // if distinct xmax points 如果有xmax点
    if (maxmax != maxmin)
    {
        // push maxmax point onto stack
        points.emplace_back();
        P[maxmax].toPointMsg(points.back());
    }
    // the bottom point of the upper hull stack
    int bot = (int) points.size();
    i = maxmin;
    while (--i >= minmax)
    {
        // the upper line joins P[maxmax]  with P[minmax]
        // ignore P[i] below or on the upper line
        if (cross(P[maxmax], P[minmax], P[i]) >= 0 && i > minmax) continue;
        // at least 2 points on the upper stack
        while ((int) points.size() > bot)
        {
            // test if  P[i] is left of the line at the stack top
            // P[i] is a new hull  vertex
            if (cross(points[points.size() - 2], points.back(), P[i]) > 0) break;
            // pop top point off stack
            points.pop_back();
        }
        // push P[i] onto stack
        points.emplace_back();
        P[i].toPointMsg(points.back());
    }
    if (minmax != minmin)
    {
        // push joining endpoint onto stack
        points.emplace_back();
        P[minmin].toPointMsg(points.back());
    }
    simplifyPolygon(polygon);
}

void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::Polygon &polygon)
{
    size_t triangleThreshold = 3;
    // check if first and last point are the same. If yes, a triangle has 4 points
    // 检查第一个点和最后一个点是否相同。如果是，三角形有4个点
    if (polygon.points.size() > 1 &&
        std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5 &&
        std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)
    {
        triangleThreshold = 4;
    }
    // nothing to do for triangles or lines 三角形和直线就不用做了
    if (polygon.points.size() <= triangleThreshold) return;
    // TODO Reason about better start conditions for splitting lines, e.g., by
    // https://en.wikipedia.org/wiki/Rotating_calipers
    polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(),
                                    parameter_.min_keypoint_separation_);;
}

void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
{
    boost::mutex::scoped_lock lock(mutex_);
    polygons_ = polygons;
}

PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
{
    boost::mutex::scoped_lock lock(mutex_);
    PolygonContainerConstPtr polygons = polygons_;
    return polygons;
}

void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig &config, uint32_t level)
{
    boost::mutex::scoped_lock lock(parameter_mutex_);
    parameter_buffered_.max_distance_ = config.cluster_max_distance;
    parameter_buffered_.min_pts_ = config.cluster_min_pts;
    parameter_buffered_.max_pts_ = config.cluster_max_pts;
    parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;
}

}//end namespace costmap_converter
